﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace SmartMathLibrary.MinPack
{
    /// <summary>
    /// For further information view the MinPack documentation.
    /// </summary>
    [Serializable]
    public static class LmparClass
    {
        /// <summary>
        /// For further information view the MinPack documentation.
        /// </summary>
        /// <param name="n">The n parameter.</param>
        /// <param name="r">The r parameter.</param>
        /// <param name="ipvt">The ipvt parameter.</param>
        /// <param name="diag">The diag parameter.</param>
        /// <param name="qtb">The QTB parameter.</param>
        /// <param name="delta">The delta parameter.</param>
        /// <param name="par">The par parameter.</param>
        /// <param name="x">The x parameter.</param>
        /// <param name="sdiag">The sdiag parameter.</param>
        /// <param name="wa1">The wa1 parameter.</param>
        /// <param name="wa2">The wa2 parameter.</param>
        public static void Lmpar(int n, double[,] r, int[] ipvt, double[] diag, double[] qtb,
                                 double delta, ref double par, double[] x, double[] sdiag, double[] wa1,
                                 double[] wa2)
        {
            double[] dpmpar = {
                                  2.220446049250313e-16,
                                  2.225073858507201e-308,
                                  1.797693134862316e-308
                              };

            int i, j, jp1, k, l;
            double fp, gnorm, parc, paru;
            double sum, temp;

            double dwarf = dpmpar[1];
            int nsing = n;
            for (j = 0; j < n; j++)
            {
                wa1[j] = qtb[j];
                if ((r[j, j] == 0.0) && (nsing == n))
                {
                    nsing = j;
                }
                if (nsing < n)
                {
                    wa1[j] = 0.0;
                }
            }
            if (nsing >= 1)
            {
                for (k = 0; k < nsing; k++)
                {
                    j = nsing - k - 1;
                    wa1[j] /= r[j, j];
                    temp = wa1[j];
                    if (j < 1)
                    {
                        continue;
                    }
                    for (i = 0; i < j; i++)
                    {
                        wa1[i] -= r[j, i] * temp;
                    }
                }
            }
            for (j = 0; j < n; j++)
            {
                l = ipvt[j];
                x[l] = wa1[j];
            }
            int iter = 0;
            for (j = 0; j < n; j++)
            {
                wa2[j] = diag[j] * x[j];
            }
            double dxnorm = EnormClass.Enorm(n, wa2);
            fp = dxnorm - delta;
            if (fp <= 0.1 * delta)
            {
                if (iter == 0)
                {
                    par = 0.0;
                }
                return;
            }
            double parl = 0.0;
            if (nsing >= n)
            {
                for (j = 0; j < n; j++)
                {
                    l = ipvt[j];
                    wa1[j] = diag[l] * wa2[l] / dxnorm;
                }
                for (j = 0; j < n; j++)
                {
                    sum = 0.0;
                    if (j >= 1)
                    {
                        for (i = 0; i < j; i++)
                        {
                            sum += r[j, i] * wa1[i];
                        }
                    }
                    wa1[j] = (wa1[j] - sum) / r[j, j];
                }
                temp = EnormClass.Enorm(n, wa1);
                parl = ((fp / delta) / temp) / temp;
            }
            for (j = 0; j < n; j++)
            {
                sum = 0.0;
                for (i = 0; i <= j; i++)
                {
                    sum += r[j, i] * qtb[i];
                }
                l = ipvt[j];
                wa1[j] = sum / diag[l];
            }
            gnorm = EnormClass.Enorm(n, wa1);
            paru = gnorm / delta;
            if (paru == 0.0)
            {
                paru = dwarf / Math.Min(delta, 0.1);
            }
            par = Math.Max(par, parl);
            par = Math.Min(par, paru);
            if (par == 0.0)
            {
                par = gnorm / dxnorm;
            }
            while (true)
            {
                iter++;
                if (par == 0.0)
                {
                    par = Math.Max(dwarf, 0.001 * paru);
                }
                temp = Math.Sqrt(par);
                for (j = 0; j < n; j++)
                {
                    wa1[j] = temp * diag[j];
                }
                QrsolvClass.Qrsolv(n, r, ipvt, wa1, qtb, x, sdiag, wa2);
                for (j = 0; j < n; j++)
                {
                    wa2[j] = diag[j] * x[j];
                }
                dxnorm = EnormClass.Enorm(n, wa2);
                temp = fp;
                fp = dxnorm - delta;

                if ((Math.Abs(fp) <= 0.1 * delta) || (parl == 0.0) && (fp <= temp)
                                                     && (temp > 0.0) || iter == 10)
                {
                    if (iter == 0)
                    {
                        par = 0.0;
                    }
                    return;
                }
                for (j = 0; j < n; j++)
                {
                    l = ipvt[j];
                    wa1[j] = diag[l] * wa2[l] / dxnorm;
                }
                for (j = 0; j < n; j++)
                {
                    wa1[j] /= sdiag[j];
                    temp = wa1[j];
                    jp1 = j + 1;
                    if (jp1 < n)
                    {
                        for (i = jp1; i < n; i++)
                        {
                            wa1[i] -= r[j, i] * temp;
                        }
                    }
                }
                temp = EnormClass.Enorm(n, wa1);
                parc = ((fp / delta) / temp) / temp;
                if (fp > 0.0)
                {
                    parl = Math.Max(parl, par);
                }
                if (fp < 0.0)
                {
                    paru = Math.Min(paru, par);
                }
                par = Math.Max(parl, par + parc);
            }
        }
    }
}